#include "PostProcessing.h"

#define IFDEBUG if(0)

void MultiTracker::drawDebug(Mat & dsp)
{
	for(int j = 0; j<obj_to_tracking;j++)
		if( if_tracker_working[j] )
		{				

				ControlPoints _map_particle = my_trackers[j].getMAP();

				drawPoints(dsp, _map_particle );				
		}

}

void MultiTracker::getPosterior( Mat & res, Size _size)
{
	res = Mat::zeros( _size, 5);

	for(int j = 0; j<obj_to_tracking;j++)
		if(if_tracker_working[j])
		{
			Mat temp;
			my_trackers[j].getPosterior(temp,_size);
			res = res + temp;
		}

	res = min(res, 1);
}

void MultiTracker::work( Mat & mask_res )
{

	for(int j = 0; j<obj_to_tracking;j++)
		if(if_tracker_working[j]<0)
			if_tracker_working[j] = 0;

	//for(int i = 0; i < obj_to_tracking ;i++)  if_tracker_working[i] = 0;

	preprocessing( mask_res );

	samplingContour( mask_res );

	updataTracker( mask_res );

	validatePartly();

	removeOverlappingTracker( );

	removeSeparate();

	num_tracker_working = 0;

	for(int i = 0; i < obj_to_tracking ;i++)
		if( if_tracker_working[i] ) num_tracker_working++;

	if( num_tracker_working < obj_to_tracking)
		newTracker( mask_res );

	num_tracker_working = 0;

	for(int i = 0; i < obj_to_tracking ;i++)
		if( if_tracker_working[i] ) num_tracker_working++;

	replaceRes();

	if( num_tracker_working == obj_to_tracking )
		replaceTracker();
}

void MultiTracker::replaceTracker()
{
	vector<float> area_post;
	int n_obj_postprocessing = (int) res_postprocessing.size();	
	area_post.resize( n_obj_postprocessing );

	for(int i = 0 ; i< n_obj_postprocessing;i++)
	{
		area_post[i] = float(- contourArea( Mat( points_postprocessing[i] ) ));		

		for(int j = 0 ; j < obj_to_tracking; j++)
			if( if_tracker_working[j] )
				if( predict(res_tracker[j],res_postprocessing[ i ]).getRecall()
					>	min(0.9f,overlapping_upper_bound ))
				{
					area_post[i] = 1.0f;
					break;
				}
	}

	vector<int> sort_post;

	lc::argsort( area_post, sort_post );

	vector<float> area_tracker;
	area_tracker.resize( obj_to_tracking );

	for(int j = 0 ; j < obj_to_tracking; j++)
		if( if_tracker_working[j] )
		{

			ControlPoints _map_particle = my_trackers[j].getMAP();

			area_tracker[j] = float(contourArea( Mat( _map_particle ) ));			
		}

	vector<int> sort_tracker;

	lc::argsort( area_tracker, sort_tracker);

	for(int u = 0 ; u < min(n_obj_postprocessing,obj_to_tracking); u++)
	{
		int j = sort_tracker[u];
		int i = sort_post[u];

		if( area_post[i] < 0.0f
			&& -area_post[i] > 1.1f * area_tracker[j] )
		{
			my_trackers[j].init( points_postprocessing[  i ] , 500);	
			res_postprocessing[ i ].copyTo( res_tracker[j] );
			cout << "bigger one replaced " << endl;
		}
	}
	

}

void MultiTracker::replaceRes()
{
	int n_obj_postprocessing = (int) res_postprocessing.size();	

	for(int j = 0; j<obj_to_tracking;j++)
		if( if_tracker_working[j] )
		{
			vector<float> sort_score;
			sort_score.resize( n_obj_postprocessing );

			for(int i = 0 ; i < n_obj_postprocessing;i++)
				sort_score[i] = -tracker_on_part[j][i].getF1();

			vector<int> sort_id;

			lc::argsort( sort_score,sort_id);
			
			if( -sort_score[sort_id[0]] > 0.65f )
			{
				//res_postprocessing[ sort_id[0] ].copyTo( res_tracker[j] );

				my_trackers[j].init( points_postprocessing[  sort_id[0] ] , 1000);					
				res_postprocessing[ sort_id[0] ].copyTo( res_tracker[j] );
			}				
		}

}



void MultiTracker::samplingContour( Mat & mask_res )
{
	//output

	sampleFromContour( mask_res, points_postprocessing, &res_postprocessing);	

	IFDEBUG
	{
		cv::destroyAllWindows();
		for(int i = 0 ;  i < (int)res_postprocessing.size();i++)
		{
			char fname[400];
			sprintf(fname,"win_%d",i);
			Mat dsp;
			cv::resize( res_postprocessing[i],dsp,Size(),.25,.25);		
			imshow(fname,dsp);
			cv::waitKey(1);
		}
		cv::waitKey(0);
	}
}

void MultiTracker::newTracker( Mat & mask_res)
{
	int n_obj_postprocessing = (int) res_postprocessing.size();	

	

	int num_require = obj_to_tracking - num_tracker_working;

	//cout << num_require << " required" << endl;

	vector<float> sort_score; sort_score.resize( n_obj_postprocessing );

	for(int i = 0; i < n_obj_postprocessing ;i++)
		//sort_score[i] = -float( points_postprocessing[i].size());
		sort_score[i] = - predict( res_postprocessing[i], mask_res).getRecall();

	vector<int> sort_id; lc::argsort( sort_score, sort_id );

	for(int i = 0; i < n_obj_postprocessing ;i++)
	{
		if(num_require==0) break;

		int chain_id = sort_id[i];

		bool if_overlapping = false;

		for(int j = 0 ; j < obj_to_tracking; j++)
			if( if_tracker_working[j] )
				if( predict(res_postprocessing[ chain_id ],res_tracker[j]).getRecall()
					>	overlapping_upper_bound)
				{
					if_overlapping = true;
					break;
				}

		if(!if_overlapping)
		{
			for(int j = 0; j<obj_to_tracking;j++)
				if( !if_tracker_working[j] )
				{
					my_trackers[j].init( points_postprocessing[ chain_id ] , 1000);
					res_postprocessing[ chain_id ].copyTo( res_tracker[j] );
					if_tracker_working[j] = 1;

					Mat dsp;
					cv::cvtColor( res_tracker[j],dsp, CV_GRAY2RGB);

					IFDEBUG
					{
						drawPoints(dsp, points_postprocessing[ chain_id ] );
						imshow("add",dsp);
						cv::waitKey(0);
						cv::destroyWindow("add");
					}

					break;
				}

			num_require--;
			num_tracker_working++;
		}
	}

}

void MultiTracker::removeOverlappingTracker( )
{
	Mat recall_mat = Mat::zeros( obj_to_tracking, obj_to_tracking, 5 );

	for(int i = 0 ; i < obj_to_tracking;i++)
		if( if_tracker_working[i] )
		for(int j = 0 ; j < obj_to_tracking; j++)
			if( if_tracker_working[j] )
			if(i!=j)			
				recall_mat.at<float>(i,j) = predict(res_tracker[i],res_tracker[j]).getRecall();

	for(int i = 0 ; i < obj_to_tracking-1 ;i++)
		if( if_tracker_working[i] )
		for(int j = i+1 ; j < obj_to_tracking; j++)
			if( if_tracker_working[j] )
				if(recall_mat.at<float>(i,j) > overlapping_upper_bound)
				{
					if( recall_mat.at<float>(i,j) > recall_mat.at<float>(j,i) )
						if_tracker_working[j]--;
					else
						if_tracker_working[i]--;
				}
				else if( recall_mat.at<float>(j,i) > overlapping_upper_bound)
				{
					if_tracker_working[i]--;					
				}
}

void MultiTracker::removeSeparate()
{
	int n_obj_postprocessing = (int) res_postprocessing.size();	

	if(n_obj_postprocessing<2) return;

	for(int j = 0; j<obj_to_tracking;j++)
		if( if_tracker_working[j] )
		{
			vector< float > partly_precision;
			partly_precision.resize( n_obj_postprocessing );

			float sum_precision = 0.0f;

			for(int i = 0; i < n_obj_postprocessing ;i++)
			{
				partly_precision[i] = - tracker_on_part[j][i].getPrecision();
				sum_precision = sum_precision + partly_precision[i];
			}

			vector< int> sort_id;

			lc::argsort( partly_precision , sort_id );

			float two_part_recall = (partly_precision[sort_id[0]] + partly_precision[sort_id[1]])/sum_precision;

			if( -sum_precision < 0.7f && two_part_recall > 0.85f && partly_precision[sort_id[0]]/partly_precision[sort_id[1]] < 3.0f )
			{
				if_tracker_working[j] = min(1, if_tracker_working[j]-- );

				IFDEBUG
				{
					imshow("try remove separate",res_tracker[j]);
					cv::waitKey(0);
					cv::destroyWindow("try remove separate");
				}
			}
		}

}

void MultiTracker::validatePartly()
{
	int n_obj_postprocessing = (int) res_postprocessing.size();	

	tracker_on_part.resize( obj_to_tracking );

	for(int j = 0; j<obj_to_tracking;j++)
	{
		tracker_on_part[j].resize( n_obj_postprocessing );
		if( if_tracker_working[j] )
		{
			for(int i = 0; i < n_obj_postprocessing ;i++)
			{
				tracker_on_part[j][i] = predict(res_tracker[j],res_postprocessing[i]);
			}
		}
		else
		{
			for(int i = 0; i < n_obj_postprocessing ;i++)
				tracker_on_part[j][i] = LcValidator(0,0,0,0);
		}			
	}
}
void MultiTracker::updataTracker( Mat & mask_res )
{

	num_tracker_working = 0; // to output

	for(int j = 0; j<obj_to_tracking;j++)
		if( if_tracker_working[j] )
		{
			Mat _normed_mask;

			mask_res.convertTo( _normed_mask, 5 );

			_normed_mask = _normed_mask * (1.0/ 255.0);

			for(int k = 0 ; k < 2 ;k++)
			{
				my_trackers[j].resample();

				my_trackers[j].motion();

				my_trackers[j].updataWeight( _normed_mask );

				vector<vector<Point> > _map_chain; _map_chain.resize(1);

				ControlPoints _map_particle = my_trackers[j].getMAP();
				_map_chain[0].resize( _map_particle.size() );
				for(int i = 0 ; i < (int) _map_particle.size();i++)
				{
					_map_chain[0][i].x = int( _map_particle[i].x );
					_map_chain[0][i].y = int( _map_particle[i].y );
				}

				res_tracker[j] = Mat::zeros(mask_res.size(), CV_8U);
				drawContours(res_tracker[j],_map_chain,0,Scalar::all(255),-1,CV_AA);

				score_tracker[j] = predict( res_tracker[j], mask_res );

				if( score_tracker[j].getPrecision() < precision_lower_bound )
				{
					if_tracker_working[j]--;
					if(if_tracker_working[j]<0) if_tracker_working[j] = 0;

					IFDEBUG
					{
						imshow("try remove precision",res_tracker[j]);
						cv::waitKey(0);
						cv::destroyWindow("try remove precision");
					}
				}
				else
				{
					IFDEBUG
					{
						Mat dsp;
						cv::cvtColor( res_tracker[j], dsp, CV_GRAY2RGB);
						drawPoints(dsp, _map_particle );
						imshow("updata",dsp);					
						cv::waitKey(0);
						cv::destroyWindow("updata");
					}
				}
			}

			num_tracker_working++;
		}

}

LcValidator MultiTracker::predict( Mat & res, Mat & lab ,unsigned char th )
{
	unsigned char * p_res = (unsigned char *) res.data;
	unsigned char * p_lab = (unsigned char *) lab.data;
	
	float tp, fp, tn, fn;
	
	tp = fp = tn = fn = 0.0f;	

	for(int sz = res.rows * res.cols ; sz>0; sz--, p_res++, p_lab++)	
	{
		if( *p_res > th)
		{
			if( *p_lab >th) tp += 1.0f;
			else fp += 1.0f;
		}
		else
		{
			if( *p_lab >th) fn += 1.0f;
			else tn += 1.0f;
		}
	}

	{
		float n = float( res.rows * res.cols );
		fp/=n; tp/=n; tn/=n; fn/=n;
		
		//return
	}

	return LcValidator( tp, fp, fn, tn);
}



//===============================

Point2f LcPoly::getPrevious(int i)
{
	if(i<=0) i += point_n;
	return controlpoints[i-1];
}

Point2f LcPoly::getNext(int i)
{
	return controlpoints[i+1];
}

Point2f LcPoly::getTangent( int i)
{
	if(i<=0) i += point_n;

	Point2f v = controlpoints[i+1] - controlpoints[i-1];

	v = v * (1.0/sqrt(v.x * v.x + v.y *  v.y ) );
	
	return v;
}

Point2f LcPoly::getNormal( int i )
{
	Point2f v = getTangent(i);

	return Point2f( -v.y , v.x);
}



LcPoly::LcPoly( ControlPoints & points)
{
	controlpoints = points;

	point_n = (int) points.size();

	controlpoints.push_back( controlpoints[0] );
	controlpoints.push_back( controlpoints[1] );
}

void LcPoly::calueCenter()
{
	center = Point2f(0,0);
	for(int i = 0 ; i < point_n;i++)
		center = center + controlpoints[i];
	center = center * (1.0/double( point_n ) );
}

//=============================

float LcSnake::combine_score( vector< float > & scores )
{
	//if p = 0.6 correct
	//then the 
	float ans = 1.0f;
	for(int i = 0 ; i < (int)scores.size() ; i++)
	{
		ans *= (scores[i] + 0.2f)/1.2f;
	}
	return ans;
}

float LcSnake::predict_point(  Mat & frm, LcPoly & contour ,int point_id, Point2f & force)
{
	force = Point2f(0,0);

	Point2f point_now = contour.controlpoints[ point_id ];

	//sampling from image

	vector< float > res;
	vector< float > res_weight;

	int sampling_length = 10;

	{
		int _rows = frm.rows; int _cols = frm.cols;
		

		float step = 1.1f;

		res.resize( sampling_length*2 );
		res_weight.resize( sampling_length*2 );

		Point2f norm = contour.getNormal( point_id );

		norm = norm * step;

		Point2f _po = point_now + norm * ( double(sampling_length) - 0.5 );

		for(int i = 0 ; i < 2*sampling_length;i++)
		{
			int r = int(_po.y);
			int c = int(_po.x);
			if(r<0 || c< 0 || r>=_rows || c>=_cols) res[i] = 0;
			else res[i] = frm.at<float>(r,c);

			_po = _po - norm;

			res_weight[i] = float(sampling_length - fabs( double(i-sampling_length)) +1);
		}

	}

	float min_error = 99.0f;

	//give force at normal direction
	{
		float mass = 0.0f;
		for(int i = 0 ; i < sampling_length*2 ; i++) mass+=res[i];

		float left_error = 0.0;

		float right_error = 0.0;
		for(int i = 0 ; i < sampling_length*2 ; i++)
			right_error += (1.0f-res[i]) * (1.0f-res[i])*res_weight[i];

		
		int min_i = 0;

		for(int i = 0 ; i < sampling_length*2; i++)
		{
			left_error -=  (1.0f-res[i]) * (1.0f-res[i])*res_weight[i];
			right_error += res[i] * res[i]*res_weight[i];
			float error_now = left_error + right_error;
			if(error_now < min_error)
			{
				min_i = i;
				min_error = error_now;
			}
		}

		Point2f norm = contour.getNormal( point_id );

		if( mass > 2.0)
			force = force + norm * double(sampling_length -  min_i) * 0.3;
		else
		{
			force = force + (contour.center -point_now ) * 0.1;
			//give force to center
		}
	}

	//give force at tagent direction
	{
		Point2f tangent = contour.getTangent( point_id );

		Point2f target = ( contour.getNext(point_id) 
			+ contour.getPrevious(point_id)) * 0.5;

		Point2f dirt = target - point_now;
		
		force = force + tangent * (dirt.x * tangent.x + dirt.y * tangent.y) * 0.8;
	}

	//mean = contour.point_n * 0.6
	//std = contour.point_n * 0.6 * 0.4
	{

		float min_error = 0;

		for(int i = 0, j =sampling_length  ; i < sampling_length; i++,j++)
		{
			min_error = min_error + res[i] * res[i];
			min_error = min_error +  (1.0f-res[j]) * (1.0f-res[j]);
		}


		float mean = float(sampling_length*2) * 0.3f;
		float std = float(sqrt(double(sampling_length*2) * 0.7 * 0.3) * 9.0);
		if( min_error < mean - std ) return 1.0f;
		else if( min_error < mean )
		{
			float a = min_error - mean + std;
			return 0.5f + 0.5f * a*a/std/std;
		}
		else if( min_error > mean + std ) return 0.0f;
		else
		{
			float a = mean + std - min_error;
			return 0.5f * a*a/std/std;
		}
	}	
}


void LcSnake::resampleChain( ControlPoints & points )
{
	ControlPoints _points = points;

	int _point_n = (int) _points.size();

	_points.push_back( _points[0] );

	float dis = 0;

	for(int i = 0 ; i < _point_n ; i++)
	{
		float x = _points[i].x - _points[i+1].x;
		float y = _points[i].y - _points[i+1].y;
		dis = dis + abs(x) + abs(y);
	}

	float step = dis/float(_point_n) + 1e-2f;

	ControlPoints resampled_chain;
	resampled_chain.clear();
	resampled_chain.reserve( _point_n );

	resampled_chain.push_back( _points[0] );

	dis = 0;
	for(int i = 0 ; i < _point_n; i++)
	{
		Point2f p0 = _points[i];
		Point2f p1 = _points[i+1];
		float x = p0.x - p1.x;
		float y = p0.y - p1.y;

		float vec = abs(x) + abs(y);
		dis = dis + vec;

		if(dis > step)
		{
			for(float fraction = step - dis + vec; fraction < vec ; fraction+= step )
				resampled_chain.push_back(p0 * ( fraction/ vec ) + p1 * ((vec-fraction) / vec));

			while(dis>step) dis-=step;
		}
	}

	if( (int)resampled_chain.size()!=_point_n)
	{
		cout << "warning!"<< endl;
		resampled_chain.resize( _point_n );
	}

	points = resampled_chain;
}

float LcSnake::predict( Mat & frm, ControlPoints & points )
{

	int dim = (int) points.size();
	

	int n_epoch = 1;// times for updating

	vector<float> scores;
	scores.resize(  dim );

	vector<Point2f> force;
	force.resize( dim );

	for(int i = 0 ; i < n_epoch; i++)
	{
		LcPoly contour(points);

		contour.calueCenter();

		for(int j = 0 ; j < dim; j++)
		{
			scores[j] = predict_point( frm , contour, j , force[j]);

			//if(j%10==0) cout << scores[j] << endl;
		}

		//do not move while observe
		//for(int j = 0 ; j < dim; j++) points[j] = points[j] + force[j];

		//if(i< n_epoch/2 ) resampleChain( points );
	}

	return combine_score( scores );
}


//================================


ContourPF::ContourPF()
{
	shape_noise = 0.5f;
	motion_noise = 10.0f;
	angle_noise = 5.0f/180.f*3.2f;
	my_observer = new LcSnake;
}

void ContourPF::init(  vector< Point2f > & first_particle, int n )
{
	particle_n = n;

	particles.resize( particle_n );

	for(int i = 0; i < particle_n ; i++)
	{
		particles[i] = first_particle;
	}

	dim_n = (int) first_particle.size();

	weights.resize( particle_n );

	for(int i = 0; i < particle_n ; i++) weights[i] = 1.0f;
}



ControlPoints ContourPF::getMAP()
{
	int max_id = 0 ; 

	for(int i = 0 ; i < particle_n ; i++)
		if( weights[i] > weights[max_id] ) max_id = i;

	//cout << weights[max_id] << endl;

	return particles[max_id];
}



void ContourPF::getPosterior( Mat & res, Size _size)
{
	float sum_weight = 0.0f;

	//Mat 

	res = Mat::zeros( _size, 5);

	for(int j = 0; j < particle_n ; j++ )
	{
		Mat temp = Mat::zeros(_size, CV_8U);

		vector<vector<Point> > _posterior_chain; _posterior_chain.resize(1);		

		_posterior_chain[0].resize( particles[j].size() );
		for(int i = 0 ; i < (int) particles[j].size();i++)
		{
			_posterior_chain[0][i].x = int( particles[j][i].x );
			_posterior_chain[0][i].y = int( particles[j][i].y );
		}

		drawContours(temp ,_posterior_chain,0,Scalar::all(255),-1,CV_AA);

		temp.convertTo( temp, 5);

		res = res + temp;

		sum_weight+=weights[j];
		
	}

	res = res * (1.0/ 255.0 / double(sum_weight));
}

void ContourPF::resample( )
{
	{
		float sum_weight = 0.0f;

		for(int i =0 ; i < particle_n;i++)
			sum_weight += weights[i];

		for(int i = 0 ; i < particle_n ; i++)
			weights[i] /= sum_weight;
	}

	{
		vector< ControlPoints > new_particles;

		new_particles.reserve( particle_n + 5 );

		float step = float( 1.0f / float( particle_n ) );

		float r = float( rand() / double( RAND_MAX ) ) * step;

		for(int i = 0 ; i < particle_n ; i++ )
		{
			r+= weights[i];
			while( r > step)
			{
				r-= step;
				new_particles.push_back( particles[i] );				
			}
		}

		while((int) new_particles.size() < particle_n ) 
			new_particles.push_back( particles[ particle_n - 1]);

		new_particles.resize( particle_n );

		for(int i = 0 ; i < particle_n ; i++)
			weights[i] = 1.0f;

		particles = new_particles;
		
	}
}

void ContourPF::updataWeight( Mat & mask )
{
	for( int i = 0 ; i < particle_n ; i++)
	{
		LcPoly temp( particles[i] );
		weights[i] = weights[i]  * my_observer->predict( mask, particles[i] );
	}

	//resample();
}

void ContourPF::motion( ){ // right now just gaussian noise
	for( int i = 0 ; i < particle_n ; i++)
	{
		Point2f shift( motion_noise * float(lc::normrnd()),motion_noise * float(lc::normrnd()) );
		Point2f center(0,0);
		
		float angle_shift = angle_noise * float(lc::normrnd());

		for(int j = 0; j < dim_n; j++) center = center + particles[i][j];
		center = center * (1.0f/float( dim_n ) );

		Point2f center_shift = center + shift;

		float _c = cos( angle_shift );
		float _s = sin( angle_shift );

		for(int j = 0; j < dim_n; j++)
		{
			float _x = particles[i][j].x - center.x +shape_noise *  float(  lc::normrnd());
			float _y = particles[i][j].y - center.y +shape_noise *  float(  lc::normrnd());

			particles[i][j].x = center_shift.x + _x * _c - _y * _s;
			particles[i][j].y = center_shift.y + _x * _s + _y * _c;
		}
	}
}



void ContourPF::draw(Mat & frm, Mat & dsp)
{
	vector<int> id;

	lc::argsort( weights, id);

	int show_n = 100;

	show_n = min(show_n , (int) id.size());

	int block_n = int(sqrt(double(show_n)));


	Mat temp_epitome; cv::resize( frm , temp_epitome, Size(), .25, .25 );
	int _rows = temp_epitome.rows;
	int _cols = temp_epitome.cols;
	dsp = Mat ::zeros( _rows * block_n , 
		_cols * block_n , frm.type() );

	for(int id = 0 ; id < show_n ; id++)
	{
		int i = id / block_n;
		int j = id % block_n;

		frm.copyTo( temp_epitome );

		drawPoints( temp_epitome, particles[id] );

		if(id==0)
		{
			IFDEBUG
			{
				imshow("debug one particle",temp_epitome);
				cv::waitKey(1);
			}
		}

		cv::resize( temp_epitome, temp_epitome , Size(), .25, .25 );

		temp_epitome.copyTo( dsp(	Range( _rows* i, _rows* (i+1) ),
									Range( _cols* j, _cols* (j+1) )) );
	}

	cv::resize( dsp, dsp, Size( 800, 600) );
	
}
//===========================================

void drawPoints( Mat & dsp, vector< Point2f > &  points)
{
	LcPoly my_poly( points );

	int r = 0;//rand()%155+100;
	int g = 0;//rand()%255+0;
	int b = 255;//rand()%255+0;

	Mat temp;
	dsp.copyTo(temp);


	for(int i = 0; i< (int) points.size(); i++)
	{
		circle(temp,points[i],2,CV_RGB(r,g,b),-1);
		circle(dsp,points[i],2,CV_RGB(r,g,b),-1);

		
		if(1) // add norm direction
		{
			Point2f normal = my_poly.getNormal( i );

			//line(dsp,points[i] + normal * 12.0,points[i] - normal * 2.0,CV_RGB(r,g,b),1,CV_AA);

			if(i!=points.size()-1)
				line(temp,points[i],points[i+1],CV_RGB(0,255,30),3,CV_AA);
			else
				line(temp,points[i],points[0],CV_RGB(0,255,30),3,CV_AA);
		}
	}

	cv::addWeighted( dsp, 0.3, temp, 0.7, 0, dsp);
}

void MultiTracker::preprocessing( Mat & mask_res )
{

	Mat _mask;

	if(mask_res.type() == CV_8U)
	{
		mask_res.convertTo( _mask, 5);
		_mask = _mask * (1.0/255.0);
	}
	else _mask = mask_res;

	Mat bin;		
	threshold(_mask, bin,0.50,255,THRESH_BINARY);
	bin.convertTo(bin,CV_8UC1);

	vector<vector<Point> > co;
	vector<Vec4i> hi;

	findContours(bin,co,hi,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE);

	Mat mask_new = Mat::zeros( _mask.size(), CV_8U);	

	if(0)
	{

		for(int i = 0 ; i < (int) co.size() ; i++)
		{
			if( contourArea( Mat( co[i] ))<1000) continue;
			if( contourArea( Mat( co[i] ))>_mask.rows * _mask.cols / 3) continue;

			drawContours( mask_new ,co,i,Scalar::all(255),-1,CV_AA);
		}
	}
	else
	{
		vector<float> sort_score;

		sort_score.resize( co.size());

		for(int i = 0 ; i < (int) co.size() ; i++)
			sort_score[i] = float(- contourArea( Mat( co[i] )) );

		vector<int> sort_id;

		lc::argsort( sort_score, sort_id );

		for(int i = 0 ; i < min(3,(int) sort_id.size()); i++)
		{
			int id = sort_id[i];
			if( - sort_score[id]<1000) continue;
			if( - sort_score[id] >_mask.rows * _mask.cols / 3) continue;
			drawContours( mask_new ,co,id,Scalar::all(255),-1,CV_AA);
		}
	}

	mask_new.copyTo( mask_res );


}

void sampleFromContour( Mat & res_mask, vector< vector<Point2f> > & points ,
					   vector< Mat > * p_contour_images)
{

	if(res_mask.type() == CV_8U)
	{
		res_mask.convertTo( res_mask, 5);
		res_mask = res_mask * (1/255.0);
	}

	Mat bin;		
	threshold(res_mask, bin,0.50,255,THRESH_BINARY);
	bin.convertTo(bin,CV_8UC1);

	vector<vector<Point> > co;
	vector<Vec4i> hi;

	findContours(bin,co,hi,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE);

	{
		int j = 0 ; 
		for(int i = 0; i < (int ) co.size();i++)
		{
			if(contourArea( Mat( co[i] ) ) < 100) continue;
			co[j] = co[i]; j++;
		}
		co.resize(j);
	}

	if(p_contour_images!= NULL ) 
	{
		vector< Mat > & images = *p_contour_images;

		images.resize( co.size() );

		for(int i = 0 ; i < (int) co.size() ; i++)
		{
			images[i] = Mat::zeros( res_mask.size() , res_mask.type() );

			drawContours(images[i],co,i,Scalar::all(255),-1,CV_AA);
		}

	}

	int max_step = 10;
	int min_n = 4;
	

	points.resize( co.size() );

	{
		int j = 0;
		for(int i = 0 ; i < (int) co.size() ; i++ )
		{
			if( co[i].size() < 5 ) continue;

			int step = (int) co[i].size() / min_n;
			if( step > max_step ) step = max_step;

			points[j].clear();
			points[j].reserve( co[i].size() / step + 4 );

			for(int k = 0; k < (int) co[i].size(); k+=step)
			{
				points[j].push_back( co[i][k]);
			}
			j++;
		}
		points.resize(j);
	}
}

void getLabelMask( Mat & res, Mat & mask , Size size, int bound_setting)
{
	int _rows = size.height;
	int _cols = size.width;

	int _sz = (_rows - 2* bound_setting) * (_cols - 2* bound_setting);

	if(_sz != (int) res.rows)
	{
		cout << "size unmatch while get label mask" << endl;
		return;
	}

	mask = Mat::zeros( _rows, _cols, CV_8U);

	vector<KeyPoint> keypts;

	{
		DenseFeatureDetector dfd;

		float initFeatureScale = 1.f;		// inital size
		int featureScaleLevels = 1;		// one level
		float featureScaleMul = 1.00f;	// multiplier (ignored if only one level)
		int train_initXyStep = 1;		// space between pixels for training (must be 1)

		dfd = DenseFeatureDetector(initFeatureScale,featureScaleLevels,featureScaleMul,train_initXyStep,bound_setting,true,false);

		dfd.detect(mask,keypts);
	}

	cout << "kp size " << keypts.size() << " res size " << res.rows << endl;
	
	for(int i = 0;i< (int)keypts.size();i++)
	{
		int r = int(floor(keypts[i].pt.y+.5));
		int c = int(floor(keypts[i].pt.x+.5));
		mask.at<unsigned char>(r,c) = (unsigned char)min( 255.0f, res.at<float>(i,0) * 255.0f * 1.5f);
	}
}